#include "ms618_algbackrcedge.h"

using namespace MeasurE618;
AlgBackRCEdge::AlgBackRCEdge(int i):AlgBase(i)
{

}

void AlgBackRCEdge::operator()(cv::Mat &srcImg,
                               int id,
                               std::map<string, float> &measRes,
                               int fixtureId)
{
//矩形边缘提取   //RC back图 \paras616\back\RC.xml ；
//int NewGetRectEdge(cv::Mat srcimg, const vector<int> paras, vector<float>& dists, vector<vector<cv::Point2f>>& contourres);

    std::vector<std::vector<cv::Point2f>> contourres;

    std::vector<int> tparas;
    int i=8,tg=0;
    for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
    {
        tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(0,j));
    }
    std::vector<cv::Rect> rangVec;
    TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
    cv::Mat imgt=srcImg(rangVec[0]).clone();

    std::vector<float> dists;
    vector<vector<cv::Point2f>> contourres0;
    edgedetect obj;
    obj.NewGetRectEdge(imgt, tparas,dists,contourres0);
    for(auto &ci:contourres0)
    {
        ImageCoordinateTransformation(ci,rangVec);
    }
    contourres.resize(contourres0.size());
    std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
    while (!AlgBase::flagVec[id])
    {
        (*AlgBase::cvarptrVec[id]).wait(lock);
    }

    for(auto &ci:contourres0)
    {

       for(auto &cj:ci)
       {
           cv::Point2f ptsOut;
//          CoordinateTransformation(datumBackVec[id],cj,ptsOut);
           CoordinateTransformationRelateToDot(datumBackVec[id], cj, ptsOut);
          contourres[tg].push_back(ptsOut);
       }
       tg++;
    }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";

    float fai20x1=0;
    for(int i=0;i<contourres[2].size();++i){
        fai20x1+=contourres[2][i].x/contourres[2].size();
    }
    float fai20x2=0;
    for(int i=0;i<contourres[4].size();++i){
        fai20x2+=contourres[4][i].x/contourres[4].size();
    }
    measRes.insert(std::make_pair(string("FAI20-RC-X1-distance_pixel"), fabs(fai20x1-fai20x2)));
    float fai20_1 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI20-RC-X1-distance")], fabs(fai20x1-fai20x2));
    measRes.insert(std::make_pair(string("FAI20-RC-X1-distance_real"), fai20_1));

    float fai20x3=0;
    for(int i=0;i<contourres[3].size();++i){
        fai20x3+=contourres[3][i].x/contourres[3].size();
    }
    float fai20x4=0;
    for(int i=0;i<contourres[5].size();++i){
        fai20x4+=contourres[5][i].x/contourres[5].size();
    }
    measRes.insert(std::make_pair(string("FAI20-RC-X2-distance_pixel"), fabs(fai20x3-fai20x4)));
    float fai20_2 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI20-RC-X2-distance")], fabs(fai20x3-fai20x4));
    measRes.insert(std::make_pair(string("FAI20-RC-X2-distance_real"), fai20_2));

    for(int i=0;i<contourres[0].size();++i){
        if(fabs(contourres[0][i].x-contourres[1][30].x)<0.5)
        {
            float fai68_1=fabs(contourres[0][i].y-contourres[1][30].y);
            measRes.insert(std::make_pair(string("FAI68-RC-Y1-distance_pixel"), fai68_1));
            float fai68_1r = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI68-RC-Y1-distance")], fai68_1);
            measRes.insert(std::make_pair(string("FAI68-RC-Y1-distance_real"), fai68_1r));
        }
        if(fabs(contourres[0][i].x-contourres[1][150].x)<0.5)
        {
            float fai68_2=fabs(contourres[0][i].y-contourres[1][150].y);
            measRes.insert(std::make_pair(string("FAI68-RC-Y2-distance_pixel"), fai68_2));
            float fai68_2r = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI68-RC-Y2-distance")], fai68_2);
            measRes.insert(std::make_pair(string("FAI68-RC-Y2-distance_real"), fai68_2r));
        }
        if(fabs(contourres[0][i].x-contourres[1][270].x)<0.5)
        {
            float fai68_3=fabs(contourres[0][i].y-contourres[1][270].y);
            measRes.insert(std::make_pair(string("FAI68-RC-Y3-distance_pixel"), fai68_3));
            float fai68_3r = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI68-RC-Y3-distance")], fai68_3);
            measRes.insert(std::make_pair(string("FAI68-RC-Y3-distance_real"), fai68_3r));
        }
        if(fabs(contourres[0][i].x-contourres[1][390].x)<0.5)
        {
            float fai68_4=fabs(contourres[0][i].y-contourres[1][390].y);
            measRes.insert(std::make_pair(string("FAI68-RC-Y4-distance_pixel"), fai68_4));
            float fai68_4r = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI68-RC-Y4-distance")], fai68_4);
            measRes.insert(std::make_pair(string("FAI68-RC-Y4-distance_real"), fai68_4r));
        }
    }

    vector<cv::Point2f> leftcontour=contourres[2];
    leftcontour.insert(leftcontour.end(),contourres[3].begin(),contourres[3].end());
    vector<cv::Point2f> rightcontour=contourres[4];
    rightcontour.insert(rightcontour.end(),contourres[5].begin(),contourres[5].end());
    float fai69 = obj.GetLine2LineMid(leftcontour, rightcontour, 1);
    measRes.insert(std::make_pair(string("FAI69-RC-X-distance_pixel"), -fai69));
    float fai69rcx = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI69-RC-X-distance")], -fai69);
    measRes.insert(std::make_pair(string("FAI69-RC-X-distance_real"), fai69rcx));

    float fai69x = m_standardValMap["FAI69-RC-X-distance"];
    float fai69PosPixelX = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI69-RC-X-distance")],
            fai69x);
    double fai69xPosPixel = 2*sqrt((fai69-fai69PosPixelX)*(fai69-fai69PosPixelX));
    measRes.insert(std::make_pair(string("FAI69-RC-X-position_pixel"), fai69xPosPixel));
    float fai69XPosDis = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI69-RC-X-position")], fai69xPosPixel);
    measRes.insert(std::make_pair(string("FAI69-RC-X-position_real"), fai69XPosDis));


    float fai70=obj.GetLine2LineMid(contourres[0],contourres[1],2);
    measRes.insert(std::make_pair(string("FAI70-RC-Y-distance_pixel"),-fai70));
    float fai70rcy = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI70-RC-Y-distance")], -fai70);
    measRes.insert(std::make_pair(string("FAI70-RC-Y-distance_real"), fai70rcy));

    float fai70y = m_standardValMap["FAI70-RC-Y-distance"];
    float fai70PosPixelY = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI70-RC-Y-distance")],
            fai70y);
    double fai70yPosPixel = 2*sqrt((-fai70-fai70PosPixelY)*(-fai70-fai70PosPixelY));
    measRes.insert(std::make_pair(string("FAI70-RC-Y-position_pixel"), fai70yPosPixel));
    float fai70yPosDis = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI70-RC-Y-position")], fai70yPosPixel);
    measRes.insert(std::make_pair(string("FAI70-RC-Y-position_real"), fai70yPosDis));

}
